#include "drivers/dev_pin.h"
#include "drv_gpio.h"
#include "dji.h"
#include "message.h"
#include "motor_def.h"
#include "offline.h"
#include "robotdef.h"
#include "rttypes.h"
#include "user_lib.h"


DJIMotor_t             *friction_l, *friction_r, *loader; // 拨盘电机
static Publisher_t     *shoot_pub;
static Shoot_Ctrl_Cmd_s shoot_cmd_recv;                   // 来自cmd的发射控制信息
static Subscriber_t    *shoot_sub;

#define LOG_TAG "shoot"
#define LOG_LVL LOG_LVL_DBG
#include <ulog.h>
void ShootInit()
{
    // 左摩擦轮
    Motor_Init_Config_s friction_config = {
        .offline_manage_motor ={
            .event = OFFLINE_SHOOT_MOTOR_LEFT,
            .enable = OFFLINE_ENABLE,
            .error_level =OFFLINE_WARNING_LEVEL,
            .beep_times = 4,
            .offline_time =1000,
        },
        .can_init_config = {
                            .can_handle = &can1_bus,
        },
        .controller_param_init_config = {
                            .lqr_config ={
                                .K ={0.07011f}, //0.0317
                                .output_max = 6.0f,
                                .output_min =-6.0f,
                                .state_dim = 1,
                            }
        },
        .controller_setting_init_config = {
                            .angle_feedback_source = MOTOR_FEED,
                            .speed_feedback_source = MOTOR_FEED,
                            .outer_loop_type    = SPEED_LOOP,
                            .close_loop_type    = SPEED_LOOP,
                            .feedback_reverse_flag =FEEDBACK_DIRECTION_NORMAL,
                            .control_algorithm =CONTROL_LQR,
        },
        .motor_type = M3508
    };
    friction_config.can_init_config.tx_id = 1,
    friction_l                            = DJIMotorInit(&friction_config);

    friction_config.can_init_config.tx_id                             = 2; // 右摩擦轮,改txid和方向就行
    friction_config.offline_manage_motor.event                        = OFFLINE_SHOOT_MOTOR_RIGHT;
    friction_config.offline_manage_motor.beep_times                   = 5;
    friction_r                                                        = DJIMotorInit(&friction_config);

    // 拨盘电机
    Motor_Init_Config_s loader_config = {
        .offline_manage_motor ={
            .event = OFFLINE_SHOOT_MOTOR_RAMMER,
            .enable = OFFLINE_ENABLE,
            .error_level =OFFLINE_WARNING_LEVEL,
            .beep_times = 6,
            .offline_time =100,
        },
        .can_init_config = {
                            .can_handle = &can1_bus,
                            .tx_id      = 3,
                            },
        .controller_param_init_config = {
                            .lqr_config ={
                                .K ={0.0034f}, //0.0317
                                .output_max = 1.8f,
                                .output_min =-1.8f,
                                .state_dim = 1,
                            }
        },
        .controller_setting_init_config = {
                            .angle_feedback_source = MOTOR_FEED,
                            .speed_feedback_source = MOTOR_FEED,
                            .outer_loop_type       = SPEED_LOOP,
                            .close_loop_type       = SPEED_LOOP,
                            .feedback_reverse_flag = FEEDBACK_DIRECTION_NORMAL,
                            .control_algorithm     = CONTROL_LQR,
        },
        .motor_type = M2006  // 英雄使用m3508

    };
    loader = DJIMotorInit(&loader_config);
    shoot_sub = SubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
    rt_pin_mode(GET_PIN(C,8),PIN_MODE_OUTPUT);
    rt_pin_write(GET_PIN(C,8),PIN_LOW);
}

/* 机器人发射机构控制核心任务 */
static void shootask(void *parameter)
{
    ShootInit();
    static rt_tick_t last_shoot_time = 0; // 记录上次发射的时间
    static rt_tick_t current_time;
    static rt_tick_t continuous_fire_time = 0;
    while (1)
    {
        // 从cmd获取控制数据
        if (offline_manage[OFFLINE_SHOOT_MOTOR_LEFT].online_state == STATE_ONLINE
            && offline_manage[OFFLINE_SHOOT_MOTOR_RIGHT].online_state == STATE_ONLINE
            && offline_manage[OFFLINE_SHOOT_MOTOR_RAMMER].online_state == STATE_ONLINE) 
        {
            SubGetMessage(shoot_sub, &shoot_cmd_recv);
            if (shoot_cmd_recv.shoot_mode ==SHOOT_ON)
            {
                DJIMotorEnable(friction_l);
                DJIMotorEnable(friction_r);
                DJIMotorEnable(loader);
                //确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
                if (shoot_cmd_recv.friction_mode == FRICTION_ON)
                {
                    // 根据收到的弹速设置设定摩擦轮电机参考值,需实测后填入
                    DJIMotorSetRef(friction_l, 6900*RPM_2_ANGLE_PER_SEC);
                    DJIMotorSetRef(friction_r, -6900*RPM_2_ANGLE_PER_SEC);
                    rt_pin_write(GET_PIN(C,8),PIN_HIGH);
                }
                else // 关闭摩擦轮
                {
                    DJIMotorSetRef(friction_l, 0);
                    DJIMotorSetRef(friction_r, 0);
                    DJIMotorSetRef(loader, 0);
                    rt_pin_write(GET_PIN(C,8),PIN_LOW);       
                }              
                switch (shoot_cmd_recv.load_mode)
                {
                    // 停止拨盘
                    case LOAD_STOP:     
                        DJIMotorSetRef(loader, 0);      
                        break;
                    // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
                    case  LOAD_1_BULLET :
                        DJIMotorSetRef(loader, -2000 * 6.0f);
                        break;
                    // 修改连发模式的逻辑
                    case LOAD_BURSTFIRE:
                        DJIMotorSetRef(loader, -6000 * 6.0f);
                        break;
                    case LOAD_REVERSE:
                        DJIMotorOuterLoop(loader, SPEED_LOOP);
                        break;
                    
                    case LOAD_3_BULLET:
                    default:
                        break;
                }
            }
            else 
            {
                DJIMotorStop(friction_l);
                DJIMotorStop(friction_r);
                DJIMotorStop(loader);
            }
        }
        else 
        {
            DJIMotorStop(friction_l);
            DJIMotorStop(friction_r);
            DJIMotorStop(loader);
        }
        //LOG_D("rest_heat:%f,%f\n",shoot_cmd_recv.rest_heat_1,shoot_cmd_recv.rest_heat_2);
        rt_thread_mdelay(5);
    }
}

int shoot_thread_entry_init(void)
{
    rt_err_t    ret    = RT_EOK;
    rt_thread_t thread = rt_thread_create("shoot", shootask, RT_NULL, 2048, 18, 10);
    /* 创建成功则启动线程 */
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
    }
    else
    {
        ret = RT_ERROR;
    }

    LOG_D("shoot thread init success!\n");

    return ret;
}

#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
INIT_APP_EXPORT(shoot_thread_entry_init);
#endif
